We first start by treating vehicles as point objects, P and Q. For the purpose of this paper, we have determined anti-clockwise rotations to be positive and clockwise rotations to be negative. We define \(\vec{u}\) and \(\vec{v}\) as the velocities of P and Q respectively:

where:

To calculate TCA of points P and Q, we find the displacement from P to Q as a function of time. Then, in order to get the time at which the displacement is minimum, we differentiate the function, equate it to 0 and solve for time. This time is the time to closest approach (TCA) of P and Q. If P and Q collide with each other, the displacement at TCA will be 0. To calculate P and Q at time t, we use the following notation:

\[\begin{equation} P(t) = \left( {\begin{array}{c} f_x(t)\\ f_y(t)\\ \end{array} } \right) \end{equation}\]

where:

Similarly:

\[\begin{equation} Q(t) = \left( {\begin{array}{c} g_x(t) \\ g_y(t) \\ \end{array} } \right) \end{equation}\]

where:

\(P(t)\) and \(Q(t)\) can be understood as the integrals of velocities \(\vec{u}\) and \(\vec{v}\) respectively over time t.

Additionally, we introduce adjusted TCA, a metric that measures the time P and Q avoided a collision by. To do so, we modify our calculation of P and Q to create \(\hat{P}\) and \(\hat{Q}\) that measure the delay in the behavior initiated at \(t_0\) that would have led to a crash. Note, while the metric is perhaps most useful in circumstances where \(t_0\) marks one vehicle’s “reaction” to a threatening situation, it remains valid when no behavior change is initiated at \(t_0\), which we refer to as a “null reaction”.

\[\begin{equation} \hat{P}(t) = \left( {\begin{array}{c} f_x(t) + u_0\cdot\cos(\theta_0)\cdot Z_P\\ f_y(t) + u_0\cdot\sin(\theta_0)\cdot Z_P\\ \end{array} } \right) \end{equation}\]

where:

\[\begin{equation} \hat{Q}(t) = \left( {\begin{array}{c} g_x(t) - v_0\cdot\sin(\phi_0)\cdot Z_Q\\ g_y(t) + v_0\cdot\cos(\phi_0)\cdot Z_Q\\ \end{array} } \right) \end{equation}\]

where:

Since we assume that points \(\hat{P}\) and \(\hat{Q}\) crash at time t with delayed reactions \(Z_P\) and \(Z_Q\), we set \(\hat{P}\) and \(\hat{Q}\) equal to each other to get \(Z_P\) and \(Z_Q\) as functions of time. Here, time t denotes the time it takes for points P and Q to collide from their initial starting points.

A Simple Example

The simplest example of our model assumes constant velocity over time:

\[\begin{equation} P(t) = P_0 + t\vec{u} \end{equation}\]
\[\begin{equation} Q(t) = Q_0 + t\vec{v} \end{equation}\]

where \(P_0\) and \(Q_0\) are positions of points P and Q at time \(t = 0\).

Define a distance vector \(\vec{d}(t)\) between P and Q as follows:

\[\begin{equation} \begin{split} \vec{d}(t) &= P(t) - Q(t) \\ &= P_0 - Q_0 + (\vec{u} - \vec{v})t \\ &= d_0 + (\vec{u} - \vec{v})t \end{split} \end{equation}\]

where \(d_0 = P_0 - Q_0\)

We calculate the TCA of P and Q by minimizing \(\vec{d}(t)\).

An animation of the scenario is as follows:

t <- 7
inc <- .1
p1 <- data.frame(x = rep(0,t/inc + 1), y = -50+10*seq(0,t, by = inc),  t = seq(0,t, by = inc))
q1 <- data.frame(x = 40 - 10*seq(0,t, by = inc), y = rep(0,t/inc + 1), t = seq(0,t, by = inc)) 
library(raster)
D <- pointDistance(p1[,-3], q1[,-3], lonlat = FALSE)
library(plotly)
df <- rbind(p1,q1)
plot1 <- df %>%
  plot_ly(
    x = ~x,
    y = ~y,
    frame = ~t,
    type = 'scatter',
    mode = 'markers',
    showlegend = F
  )
plot1

The TCA of the two points in this animation is:

TCA1 <- which.min(D)
tca1 <- seq(0,t, by = inc)[TCA1]
tca1
## [1] 4.5

Finally, the adjusted TCA gives us the graph:

Zero <- data.frame(x = rep(0,t/inc + 1), y = rep(0,t/inc + 1))
DP <- pointDistance(p1[,-3], Zero, lonlat = FALSE)/rep(10, t/inc + 1)
DQ <- pointDistance(q1[,-3], Zero, lonlat = FALSE)/rep(-10, t/inc + 1)
plot(DP,DQ)

The area enclosed between (0, 1) and (-1, 0) is the measure of safety from collision. The line from (0, 1) to (-1, 0) depicts all possible combinations of durations that points P and Q could have delayed their reactions by in order to just collide. Hence, any point in the area enclosed by the line and the x and y axes is the combination of delayed reaction time of P and Q that would have resulted in the avoidance of a collision. Note, in the event that P and Q collide, the area would simply be a single point- (0, 0).

Variable velocity with constant acceleration

We will modify our calculation of \(\vec{u}\) as follows:

Adding acceleration to each component of velocity, we get:

\[\begin{equation} \vec{u}(t) = (u_0 \cos(\theta) + a_{P0}\cos(\theta)t)\hat{i} + (u_0\sin(\theta) + a_{P0}\sin(\theta)t)\hat{j} \end{equation}\]

Similarly,

\[\begin{equation} \vec{v}(t) = -(v_0 \sin(\phi) + a_{Q0}\sin(\phi)t)\hat{i} + (v_0\cos(\phi) + a_{Q0}\cos(\phi)t)\hat{j} \end{equation}\]

An animation of the scenario is as follows:

p2 <- data.frame(x = rep(0,t/inc + 1), y = -25+10*seq(0,t, by = inc)-seq(0,t, by = inc)^2,  t = seq(0,t, by = inc))
q2 <- data.frame(x = 40 - 10*seq(0,t, by = inc), y = rep(0,t/inc + 1), t = seq(0,t, by = inc)) 
D <- pointDistance(p2[,-3], q2[,-3], lonlat = FALSE)
df <- rbind(p2,q2)
plot2 <- df %>%
  plot_ly(
    x = ~x,
    y = ~y,
    frame = ~t,
    type = 'scatter',
    mode = 'markers',
    showlegend = F
  )
plot2

The TCA of the two points in this animation is:

TCA2 <- which.min(D)
tca2 <- seq(0,t, by = inc)[TCA2]
tca2
## [1] 4

Finally, the adjusted TCA gives us the graph:

Zero <- data.frame(x = rep(0,t/inc + 1), y = rep(0,t/inc + 1))
DP <- pointDistance(p2[,-3], Zero, lonlat = FALSE)/rep(10, t/inc + 1)
DQ <- pointDistance(q2[,-3], Zero, lonlat = FALSE)/rep(-10, t/inc + 1)
plot(DP,DQ)

Non-zero angle of motion- fixed steering wheel angle

We use the steering wheel angle to determine the turn of wheels using the steering ratio. So, turn of wheels \(=\) steering wheel angle \(\times\) steering ratio.

Since turn of the wheel is the angle by which a vehicle turns at every instant, we interpret it as the derivative of a vehicle’s angle of motion.

Let turn of wheel of point P be \(W_P\). Then:

\[\begin{equation} W_P = \frac{d\theta}{dt} \end{equation}\] Since we assume that \(W_P\) is a constant, upon integrating both sides of equation (6) with respect to t, we get the angle of motion as a function of time as follows: \[\begin{equation} \theta(t) = W_P \cdot t + \theta_0 \end{equation}\]

where \(\theta_0\) is the angle at which P was turned at time \(t = 0\).

Given a fixed steering wheel angle, point P travels with velocity \(\vec{u}\) in a circular motion.

An animation of the scenario is as follows:

p3 <- data.frame(x = -300/pi + 300/pi*cos(pi*seq(0,t, by = inc)/30), y = -150/pi+300/pi*sin(pi*seq(0,t,by=inc)/30),  t = seq(0,t, by = inc))
q3 <- data.frame(x = 40 - 10*seq(0,t, by = inc), y = rep(0,t/inc + 1), t = seq(0,t, by = inc)) 
D <- pointDistance(p3[,-3], q3[,-3], lonlat = FALSE)
df <- rbind(p3,q3)
plot3 <- df %>%
  plot_ly(
    x = ~x,
    y = ~y,
    frame = ~t,
    type = 'scatter',
    mode = 'markers',
    showlegend = F
  )
plot3

The TCA of the two points in this animation is:

TCA3 <- which.min(D)
tca3 <- seq(0,t, by = inc)[TCA3]
tca3
## [1] 5.1

Finally, the adjusted TCA gives us the graph:

Zero <- data.frame(x = rep(-12.7936,t/inc + 1), y = rep(0,t/inc + 1))
DP <- pointDistance(p3[,-3], Zero, lonlat = FALSE)/rep(10, t/inc + 1)
DQ <- pointDistance(q3[,-3], Zero, lonlat = FALSE)/rep(-10, t/inc + 1)
plot(DP,DQ)

Now, we run our metric on one of the cases from the data set received from NADS:

library(R.matlab) # Reads matlab files
library(stringr) # String manipulation
library(changepoint)

# NOTE: should have the folder "MatFiles" downloaded and extracted to the location below
# Should have the files "disposition.xls" and "randomization.xlsx" in locations below


## Read in the first file (File name is 20171101161320)
m1 <- readMat(("H:\\V2VMAT\\MatFiles\\20171101161320.mat"))
part1speed <- m1$elemDataI[[which(rownames(m1$elemDataI) == 'VDS.Veh.Speed')]]
part1time <- m1$elemDataI[[which(rownames(m1$elemDataI) == 'Time')]]
part1heading <- m1$elemDataI[[which(rownames(m1$elemDataI) == 'VDS.Veh.Heading')]]
part1logstream <- m1$elemDataI[[which(rownames(m1$elemDataI) == 'SCC.LogStreams')]][,1]

df1 <- data.frame("time" =  part1time,"speed" = part1speed,"heading" = part1heading, "logstream" = part1logstream)

incdf1 <- df1[which(df1$logstream != 0),names(df1) %in% c("time","speed","heading","logstream")]

incdf1$time <- incdf1$time - incdf1$time[1]
incdf1$speed <- 0.44704*incdf1$speed

##heading angle from degree to radian and set facing direction to be 0
incdf1$heading <- incdf1$heading*pi/180
incdf1$heading <- incdf1$heading - pi


#8 is time at which logstream ==3-- will be automated later
#position of point P
for (i in 1:length(incdf1$time)){
  if (i!=1){
    incdf1$posyp[i] <- incdf1$speed[i]/60*cos(incdf1$heading[i])+incdf1$posyp[i-1]
    incdf1$posxp[i] <- incdf1$speed[i]/60*-sin(incdf1$heading[i])+incdf1$posxp[i-1]
  }
  else{
    incdf1$posyp[i] <- incdf1$speed[i]/60*cos(incdf1$heading[i]) - 8*incdf1$speed[i]
    incdf1$posxp[i] <- incdf1$speed[i]/60*-sin(incdf1$heading[i])
  }
}

#position of point Q
incdf1$posxq <- (8 - incdf1$time) *15.6464
incdf1$posyq <- incdf1$time*0

p <- data.frame(x = incdf1$posxp, y = incdf1$posyp, t = incdf1$time)
q <- data.frame(x = incdf1$posxq, y = incdf1$posyq, t = incdf1$time)

library(plotly)
df <- rbind(p,q)
pl <- df %>%
  plot_ly(
    x = ~x,
    y = ~y,
    frame = ~t,
    type = 'scatter',
    mode = 'markers',
    showlegend = F
  )
  
pl
Zero <- data.frame(x = 0, y = 0)
DP <- pointDistance(incdf1[ , 6:5], Zero, lonlat = FALSE)/incdf1$speed[1]
DQ <- pointDistance(incdf1[ , 7:8], Zero, lonlat = FALSE)/15.6464
plot(DP,DQ)

result base on prediction

  incdf1 <- df1[which(df1$logstream != 0 & df1$logstream != 3),names(df1) %in% c("time","speed","heading","logstream")]

incdf1$speed <- 0.44704*incdf1$speed
for (j in 1:length(incdf1$time)) {
  if (j ==1) {
    incdf1$acceleration[j] = 0
  }
  else{
    incdf1$acceleration[j] <- (incdf1$speed[j] -  incdf1$speed[j-1])  * 60
  }
}

##heading angle from degree to radian and set facing direction to be 0
incdf1$heading <- incdf1$heading*pi/180
incdf1$heading <- incdf1$heading - pi

try <- cpt.meanvar(incdf1$acceleration)
incdf1 <- incdf1[attr(try, "cpts")[1]+1:nrow(incdf1),]
incdf1 <- na.omit(incdf1)
incdf1$time <- incdf1$time - incdf1$time[1]

time_end <- incdf1$time[nrow(incdf1)]+1/60
#position of point P

df2 <- data.frame(time = 0:10)
df2$posxp <- df2$time * 0
time_stop <- incdf1$speed[1]/3
for(k in 1:nrow(df2)){
  if (df2$time[k] <= time_stop){
    df2$posyp[k] <- -time_end*incdf1$speed[1]+ df2$time[k]*incdf1$speed[1] - 3/2*df2$time[k]^2
     }
  else{
    df2$posyp[k] <- -time_end*incdf1$speed[1]+ time_stop*incdf1$speed[1] - 3/2*time_stop^2
  }
}

#position of point Q
df2$posxq <- (time_end - df2$time) *15.6464
df2$posyq <- df2$time*0

p <- data.frame(x = df2$posxp, y = df2$posyp, t = df2$time)
q <- data.frame(x = df2$posxq, y = df2$posyq, t = df2$time)
pq <- rbind(p, q)
pq %>% plot_ly(
  x = ~x,
  y = ~y,
  frame = ~t,
  type = 'scatter',
  mode = 'markers',
  showlegend = F
)
for(k in 1:nrow(df2)){
  if (time_end <= time_stop){
    Zp <- (time_end*incdf1$speed[1]+ time_end*incdf1$speed[1] - 3/2*time_end^2)/incdf1$speed[1]
  }
  else{
    Zp <- (time_end*incdf1$speed[1]+ time_stop*incdf1$speed[1] - 3/2*time_stop^2)/incdf1$speed[1]
  }
}
Zp
## [1] 7.643927